clc
clear
close all

%% init

param.a=1.446;
param.b=1.408;
param.c=1.4370;
param.I=1549.034;
param.m=2220;
param.g=9.81;

SideLen=1;
step=0.01;
Side=0:step:SideLen;
revSide=SideLen:-step:0;

len=length(Side);
param.ref=[zeros(2,len),zeros(2,len*2);...%,zeros(2,len),zeros(2,len),zeros(2,len),zeros(2,len*2);...
    zeros(1,len),zeros(1,len*2);...%,(pi/2)*ones(1,len),pi*ones(1,len),(3/4*pi)*ones(1,len),zeros(1,len*2);
    zeros(1,len),zeros(1,len*2);...%,zeros(1,len),zeros(1,len),zeros(1,len),zeros(1,len*2);...
    zeros(1,len),zeros(1,len*2);...%,Side,ones(1,len)*Side(end),revSide,zeros(1,len*2);...
    Side,ones(1,len*2)*Side(end)]%,ones(1,len)*Side(end),revSide,zeros(1,len),zeros(1,len*2)];

param.diagQ=[zeros(1,2),1000,0,ones(1,2)*10000];
param.diagH=[zeros(1,6)];
param.diagR=[100,1,ones(1,4)];
param.Tp=100;
param.Tvz=100;
param.lb=[-25;0;-ones(4,1)];
param.ub=[25;0;ones(4,1)];
param.boundLocation=[];
param.Block=[];

x=[0;1;0;0;0;0];
u=[0;0;1;1;1;1];
%% sim

[t,y]=ode45(@(t,y)carFceSol(t,y,u,param),0:0.01:1,x);

%% vykresleni

figure
hold on
plot(y(:,6),y(:,5))
%plot(param.ref([6],1:length(t)),param.ref([5],1:length(t)),'r');
grid on

[A,B,C,D]=carLin(x,u,param);

A*x+B*u
